/*=+--+=#=+--     Unmanned Aerial System Management Software      --+=#=+--+=#*\
|          Copyright (C) 2011 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |

     This program is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License version 2 as
     published by the Free Software Foundation.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.

            Jack Elston                       Cory Dixon                        
|           elstonj@colorado.edu              dixonc@colorado.edu              |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/

/*!
  \file fpManager.cxx
  \author Cory Dixon
*/

#include <stdlib.h>
#include <stdio.h>

#include "fpManager.h"
#include "vcConfig.h"
#include "color.h"

#define EDIT_REMOTE_FP

extern GuiStruct guiStr;
extern FifoMsg_t  * fifo;

/*<---Global Variables---->*/
static GtkActionEntry fp_entries[] = 
{	

	{ "EditFP", GTK_STOCK_EDIT,     /* name, stock id */
		"_Edit FP", NULL,            /* label, accelerator */
		"Edit the selected waypoint",      /* tooltip */ 
		G_CALLBACK (FPManager::edit_fp_cb) },

	{ "CopyFP", GTK_STOCK_COPY,     /* name, stock id */
		"_Copy FP", NULL,            /* label, accelerator */
		"Send the selected flight plan",      /* tooltip */ 
		G_CALLBACK (FPManager::copy_fp_cb) },

	{ "SendFP", GTK_STOCK_GO_UP,     /* name, stock id */
		"_Send FP", NULL,            /* label, accelerator */
		"Send the selected flight plan",      /* tooltip */ 
		G_CALLBACK (FPManager::send_fp_cb) },

	{ "SaveFP", GTK_STOCK_SAVE,     /* name, stock id */
		"_Save FP", NULL,            /* label, accelerator */
		"Save the flight-plan",      /* tooltip */ 
		G_CALLBACK (FPManager::save_fp_cb) },

	{ "ClearFP", GTK_STOCK_CANCEL,     /* name, stock id */
		"_Clear FP", NULL,            /* label, accelerator */
		"Clear the flight-plan",      /* tooltip */ 
		G_CALLBACK (FPManager::clear_fp_cb) }
};

static guint fp_n_entries = G_N_ELEMENTS (fp_entries);

FPManager::TreeStruct_t FPManager::wp_entries[] = {
	{COL_INDEX,          "Index",           G_TYPE_UCHAR}, // index
	{COL_NEXT,           "Next",            G_TYPE_UCHAR}, // next
	{COL_LAT,            "Lat [deg]",       G_TYPE_FLOAT}, // lat
	{COL_LON,            "Lon [deg]",       G_TYPE_FLOAT}, // lon
	{COL_ALT,            "Alt [m]",         G_TYPE_FLOAT}, // alt
	//{COL_WINDINTERVAL,   "Wind [s]",        G_TYPE_FLOAT}, // wind
	//{COL_PRETURN,        "Preturn",         G_TYPE_BOOLEAN}, // preturn
	//{COL_LIGHTS,         "Lights",          G_TYPE_BOOLEAN}, // lights
	//{COL_SLOPE,          "Slope",           G_TYPE_BOOLEAN}, // slope
	//{COL_LAND,           "Land",            G_TYPE_BOOLEAN}, // land
	//{COL_PARACHUTE,      "Parachute",       G_TYPE_BOOLEAN}, // parachute
	//{COL_DROP,           "Drop",            G_TYPE_BOOLEAN}, // drop
	//{COL_CAMERA,         "Camera",          G_TYPE_BOOLEAN}, // camera
	//{COL_HOVER,          "Hover",           G_TYPE_BOOLEAN}, // hover
	{COL_ORBITDIRECTION, "Orbit Dir",       G_TYPE_BOOLEAN}, // orbit dir
	{COL_ORBITRADIUS,    "Orbit Rad [m]",   G_TYPE_FLOAT}, // orbit radius
	//{COL_ORBITTIME,      "Orbit Time [s]",  G_TYPE_FLOAT}, // orbit time
	//{COL_ORBITABOVEALT,  "Orbit Above [m]", G_TYPE_FLOAT}, // orbit above
	//{COL_ORBITBELOWALT,  "Orbit Below [m]", G_TYPE_FLOAT}, // orbit below
	//{COL_USER,           "User",            G_TYPE_INT}, // user
};


/*<-End Global Variables-->*/

FPManager::FPManager() : LayerManager()
{
	for( int i=0; i <= MAX_CLIENTS; i++) {
		wpPlans[i].plan.clear_list();
		//wpPlans[i].draw = false;
		//wpPlans[i].scale = 1.0;

		for( int j = 0; j < NUM_WP_POINTS; j++) {
			wpPlans[i].imgPoints[j].line_next = NULL;
			for( int k = 0; k < MAX_PREV_LINE; k++)
				wpPlans[i].imgPoints[j].line_prev[k] = NULL;
		}
	}

	pointWidth = 5.0;
	lineWidth  = 2.0;

	drag_client   = -1;
	drag_point    = -1;
	dragging      = false;
	creating_plan = false;
	last_theta    = NAN;

	menu = NULL;
	action_group = NULL;
	menu_manager = NULL;
	list_model = NULL;
	list_view  = NULL;

	fps_list_store = NULL;
	fps_treeview = NULL;
	to_treeview = NULL;
	lc_treeview = NULL;
}

FPManager::~FPManager()
{
	if( list_model ) g_object_unref (list_model);
	if( list_view ) g_object_unref (list_view);
}

void FPManager::init(GPSMap *map)
{
	LayerManager::init(map);
	layerG = GOO_CANVAS_ITEM(gps_map->getLayerG(GPSMap::FP_LAYER));

	// Create new GtkActionGroup and add entries 
	action_group = gtk_action_group_new ("FPActions");
	gtk_action_group_add_actions (action_group, fp_entries, fp_n_entries, this);

	// insert action group into menu_manager
	menu_manager = gps_map->get_MenuManager();
	gtk_ui_manager_insert_action_group (menu_manager, action_group, GPSMap::FP_LAYER);

}

bool FPManager::read_from_cmd(int clientInd, uint8_t * buf)
{
	//-- first clear any load flight plan items for this client
	clear_client_canvas(clientInd);

	if( ! wpPlans[clientInd].plan.read_from_cmd(buf) ) 
		return false;

	create_canvas_flightplan(clientInd);

	return true;
}

// set flight plan color to show modified
void FPManager::setModified(int clientInd, int p_ind)
{
	//cout << "FPManager::setModified()" << endl;
	
	// loop through clients 
	Client * a_client;
	int i = clientInd;

	if( (a_client = guiStr.shm_ptr->clients.getClient(i)) != NULL) {
		if(wpPlans[i].wp_group)  {

			WPMask_t Mask;
			int wp_ind = wpPlans[clientInd].plan.find_first_in_plan(p_ind);
			int numPoints = wpPlans[clientInd].plan.GetPointsInPlan(Mask, wp_ind);

			// Search the list forward
			int count = 0;
			while(wp_ind < NUM_WP_POINTS && count < numPoints)
			{
				SetWPMaskBit(wpPlans[i].editing_mask,wp_ind);

				// previous line segments
				for( int p_ind = 0; p_ind < MAX_PREV_LINE; p_ind++) {
					if( wpPlans[i].imgPoints[wp_ind].line_prev[p_ind] )  {

						int thisInd = -1, nextInd = -1;
						thisInd = GPOINTER_TO_UINT(g_object_get_data (G_OBJECT (wpPlans[i].imgPoints[wp_ind].line_prev[p_ind]), "this_ind"));
						nextInd = GPOINTER_TO_UINT(g_object_get_data (G_OBJECT (wpPlans[i].imgPoints[wp_ind].line_prev[p_ind]), "next_ind"));

						g_object_set (G_OBJECT (wpPlans[i].imgPoints[wp_ind].line_prev[p_ind]), "stroke_color", "red", NULL);
					}
				}

				// orbits
				if( wpPlans[i].imgPoints[wp_ind].orbit ) {
					g_object_set (G_OBJECT (wpPlans[i].imgPoints[wp_ind].orbit), "stroke_color", "red", NULL);
				}

				// Go forward through the list
				wp_ind = wpPlans[clientInd].plan.get_waypoint(wp_ind).Next;
				count++;
			}
		}
	}
	
	return;
}

// The flight plans don't ned to be udated by a timeout function as 
// they are only changed on user events or downloaded plans from vehicles,
// which will both cause their appropriate update methods
void FPManager::updateMap()
{
	//cout << "FPManager::updateMap()" << endl;
	
	// loop through clients 
	Client * a_client;
	int i = 0;
	while( (a_client = guiStr.shm_ptr->clients.getClient(i)) != NULL) {
		if(a_client->state.gps && wpPlans[i].wp_group)  {
			uint8_t wp = a_client->telemetry.navigation.waypt;
			if( wpPlans[i].current_wp != wp ) {
				wpPlans[i].last_wp = wpPlans[i].current_wp;
				wpPlans[i].current_wp = wp;
			}

			for( int wp_ind = 0; wp_ind < NUM_WP_POINTS; wp_ind++) {

				if( !GetWPMaskBit(wpPlans[i].editing_mask,wp_ind) ) {
					// previous line segments
					for( int p_ind = 0; p_ind < MAX_PREV_LINE; p_ind++) {
						if( wpPlans[i].imgPoints[wp_ind].line_prev[p_ind] )  {

							int thisInd = -1, nextInd = -1;
							thisInd = GPOINTER_TO_UINT(g_object_get_data (G_OBJECT (wpPlans[i].imgPoints[wp_ind].line_prev[p_ind]), "this_ind"));
							nextInd = GPOINTER_TO_UINT(g_object_get_data (G_OBJECT (wpPlans[i].imgPoints[wp_ind].line_prev[p_ind]), "next_ind"));

							if( thisInd == wpPlans[i].last_wp && nextInd == wpPlans[i].current_wp )
								g_object_set (G_OBJECT (wpPlans[i].imgPoints[wp_ind].line_prev[p_ind]), "stroke_color", "cyan", NULL);
							else
								g_object_set (G_OBJECT (wpPlans[i].imgPoints[wp_ind].line_prev[p_ind]), "stroke_color", "black", NULL);
							
						}
					}

					// orbits
					if( wpPlans[i].imgPoints[wp_ind].orbit ) {
						if( wp == wp_ind )
							g_object_set (G_OBJECT (wpPlans[i].imgPoints[wp_ind].orbit), "stroke_color", "cyan", NULL);
						else
							g_object_set (G_OBJECT (wpPlans[i].imgPoints[wp_ind].orbit), "stroke_color", "black", NULL);
					}
				}
			}
		}

		// next client
		i++;
	}
	
	return;
}

bool FPManager::add_flightplan(int clientInd, String wpfile)
{
	bool valid_fp = 0;

	//-- first clear any canvas flight plan items for this client
	clear_client_canvas(clientInd);

	// load the plan into requeted client
	valid_fp = wpPlans[clientInd].plan.read_from_file(wpfile);
	if( !valid_fp ) {
		printf("Could not load waypoint plan from file: %s\n", wpfile.c_str());
		return false;
	}

	create_canvas_flightplan(clientInd);

	return valid_fp;
}

bool FPManager::send_flightplan(int clientInd, int toInd, int pointInd, int wpStartInd, float planAlt)
{
	if( clientInd < 0 || clientInd > MAX_CLIENTS)
		return false;

	pointInd = wpPlans[clientInd].plan.find_first_in_plan(pointInd);

	if( pointInd < 0 ) {
		printf("Waypoint plan does not exists\n");
		return false;
	}

	Client * me = guiStr.shm_ptr->clients.getClient(0);
	Client * to = guiStr.shm_ptr->clients.getClient(toInd);

	Waypt_t * waypoint = (Waypt_t*)CMD_DATAPTR(fifo->data);

	WPMask_t Mask;
	int numPoints = wpPlans[clientInd].plan.GetPointsInPlan(Mask, pointInd);

	// Search the list forward
	int i = 0;
	int pInd = pointInd;
	while(pInd < NUM_WP_POINTS && i < numPoints)
	{
		waypoint[i] = wpPlans[clientInd].plan.get_waypoint(pInd).getWP_t();

		// Go forward through the list, do this before we modify the point
		pInd = waypoint[i].next;

		// if we are supplied the wpStartInd, shift all waypoint numbers
		if( wpStartInd > 0 ) {
			waypoint[i].number += wpStartInd;
			waypoint[i].next   += wpStartInd;
		}

		// if we are supplied the planAltitude, set the altitude of the plan
		if( planAlt > 0 ) {
			waypoint[i].pos.alt = planAlt;
		}

		// increment counting index
		i++;

	}// While still searching points forward

	CMD_CMD(fifo->data)  = CMD_FP;
	CMD_NUM(fifo->data)  = numPoints*sizeof(Waypt_t);
	CMD_CSUM(fifo->data) = 0;
	CMD_CSUM(fifo->data) = C_calcCheckSum((uint8_t*)fifo->data);

	// send the command to myself, for the thread that handles piccolo interface
	fifo->size        = CMD_SIZE(fifo->data);
	fifo->src.s_addr  = guiStr.shm_ptr->clients.getAddr(me, to).s_addr;
	if(fifo->src.s_addr == INADDR_ANY)
		fifo->src.s_addr  = guiStr.shm_ptr->clients.getAddr(me).s_addr;
	fifo->dest.s_addr = guiStr.shm_ptr->clients.getAddr(to).s_addr;

	guiStr.vcgui->writeFifo(ThreadedPipe::INBOARD_TX);

	cout << "<-- Sent CMD_FP: dest=" << to->name << " numPoints=" << numPoints << " -->" << endl;

	return true;
}

bool FPManager::save_flightplan(int clientInd, int pointInd, String wpfile)
{
	if( clientInd < 0 || clientInd > MAX_CLIENTS)
		return false;

	pointInd = wpPlans[clientInd].plan.find_first_in_plan(pointInd);

	if( pointInd < 0 ) {
		printf("Waypoint plan does not exists\n");
		return false;
	}

	cout << "Saving waypoint plan to file: client=" << clientInd << " starting=" << pointInd << endl;

	bool valid_fp = 0;
	valid_fp = wpPlans[clientInd].plan.save_to_file(wpfile, pointInd);
	if( !valid_fp )
		printf("Could not save waypoint plan to file: %s\n", wpfile.c_str());

	return valid_fp;
}

void FPManager::translate_from_zoomChange(double zoom_factor)
{
	GPSWaypoint_t gpsPt;
	double x,y,w,h;
	GooCanvasPoints *points = NULL;

	//cout << "FPManager::translate_from_zoomChange - zoom_factor=" << zoom_factor << endl;

	//--  loop through all clients flight plans --//
	for( int fpInd = 0; fpInd < MAX_CLIENTS+1; fpInd++){
		//--  loop through and convert all lat/lon points to image within flightplans --//
		for( int wpInd = 0; wpInd < NUM_WP_POINTS; wpInd++ ) {
			if( wpPlans[fpInd].imgPoints[wpInd].pointG ) {
				// get lat/lon coordinates
				gpsPt = wpPlans[fpInd].plan.get_waypoint(wpInd);
				x = gpsPt.Lon;
				y = gpsPt.Lat;

				// transform into window
				gps_map->geoToLayer(GPSMap::FP_LAYER, x, y);

				// save to array
				wpPlans[fpInd].imgPoints[wpInd].x = x;
				wpPlans[fpInd].imgPoints[wpInd].y = y;

				g_object_set (wpPlans[fpInd].imgPoints[wpInd].pointG, "x", x, "y", y, NULL);

				if( wpPlans[fpInd].imgPoints[wpInd].orbit ) {
					g_object_get(wpPlans[fpInd].imgPoints[wpInd].orbit, 
							"radius-x", &w, "radius-y", &h, NULL);
					g_object_set(wpPlans[fpInd].imgPoints[wpInd].orbit, 
							"radius-x", w * zoom_factor, "radius-y", h * zoom_factor, NULL);
				}


				if( wpPlans[fpInd].imgPoints[wpInd].line_next ) {
					points = NULL;
					g_object_get (wpPlans[fpInd].imgPoints[wpInd].line_next, "points", &points, NULL);
					if( points ) {
						points->coords[0] = x;
						points->coords[1] = y;
						g_object_set (wpPlans[fpInd].imgPoints[wpInd].line_next, "points", points, NULL);
						goo_canvas_points_unref(points);
					}
				}

				for( int p_ind = 0; p_ind < MAX_PREV_LINE; p_ind++) {
					points = NULL;
					if( wpPlans[fpInd].imgPoints[wpInd].line_prev[p_ind] ) {
						g_object_get (wpPlans[fpInd].imgPoints[wpInd].line_prev[p_ind], "points", &points, NULL);
						if( points ) {
							points->coords[2] = x;
							points->coords[3] = y;
							g_object_set (wpPlans[fpInd].imgPoints[wpInd].line_prev[p_ind], "points", points, NULL);
							goo_canvas_points_unref(points);
						}
					}
				}
			}
		}
	}
}

void FPManager::create_canvas_flightplan(int clientInd)
{
	GPSWaypoint_t gpsPt;
	double x,y;

	//cout << "FPManager::create_canvas_flightplan - clientInd=" << clientInd << " fpG=" << layerG << endl;

	//--  loop through and convert all lat/lon points to image --//
	for( int i = 0; i < NUM_WP_POINTS; i++ ) {
		// get lat/lon coordinates
		gpsPt = wpPlans[clientInd].plan.get_waypoint(i);
		x = gpsPt.Lon;
		y = gpsPt.Lat;

		// transform into window
		gps_map->geoToLayer(GPSMap::FP_LAYER, x, y);

		// save to array
		wpPlans[clientInd].imgPoints[i].x = x;
		wpPlans[clientInd].imgPoints[i].y = y;
	}

	String stroke_color = "black";
	if( clientInd == MAX_CLIENTS )
		stroke_color = "blue";

	// Create group to hold enture flight plan
	if( wpPlans[clientInd].wp_group == NULL) 
	{
		wpPlans[clientInd].wp_group = goo_canvas_group_new (GOO_CANVAS_ITEM(layerG), NULL);

		String nameStr;
		if( clientInd == MAX_CLIENTS )
			nameStr = "Local";
		else {
			Client *a_client = guiStr.shm_ptr->clients.getClient(clientInd);
			if( a_client )
				nameStr = a_client->name;
			else {
				char name[32];
				sprintf(name, "Client-%i",clientInd);
				nameStr = name;
			}
		}
		// make sure layer is shown
		gps_map->addLayer( nameStr, wpPlans[clientInd].wp_group, (GooCanvasItem*)GPSMap::FP_LAYER);
		gps_map->setLayerListShown( wpPlans[clientInd].wp_group, 1);

		// make sure parent manager layer is shown
		gps_map->toggleLayerShown(GPSMap::FP_LAYER, true);
	}

	// loop through and draw the connectors for the flight plans 
	WPMask_t mask, plan_mask;
	SetAllWPMaskBits(mask);

	int32_t i = FindFirstWPMaskBitSet(mask);
	while(i >= 0)
	{
		// is this point a member of a waypoint plan
		ClearAllWPMaskBits(plan_mask);
		int numPoints = wpPlans[clientInd].plan.GetPointsInPlan(plan_mask, i);
		if( numPoints > 0 )
		{
			// now loop through plan and draw lines
			int32_t j = FindFirstWPMaskBitSet(plan_mask);

			while(j >= 0)
			{
				gpsPt = wpPlans[clientInd].plan.get_waypoint(j);
				int ind = gpsPt.index;
				if( ind < NUM_WP_POINTS && gpsPt.IsPointValid() ) {
					int thisInd = gpsPt.index;
					int nextInd = gpsPt.Next;

					ClearWPMaskBit(wpPlans[clientInd].editing_mask, thisInd);

					// get next waypoint
					GPSWaypoint_t gpsPt_next = wpPlans[clientInd].plan.get_waypoint(nextInd);

					//-- draw line to next next waypoint --//
					if( gpsPt_next.index == nextInd && gpsPt_next.IsPointValid() )
					{
						wpPlans[clientInd].imgPoints[thisInd].line_next = goo_canvas_polyline_new_line( wpPlans[clientInd].wp_group,
								wpPlans[clientInd].imgPoints[thisInd].x, wpPlans[clientInd].imgPoints[thisInd].y,
								wpPlans[clientInd].imgPoints[nextInd].x, wpPlans[clientInd].imgPoints[nextInd].y,
								"line-width", lineWidth,
								"stroke_color", stroke_color.c_str(),
								"end-arrow", TRUE,
								NULL);

						g_object_set_data (G_OBJECT (wpPlans[clientInd].imgPoints[thisInd].line_next), 
								"this_ind", GINT_TO_POINTER(thisInd));
						g_object_set_data (G_OBJECT (wpPlans[clientInd].imgPoints[thisInd].line_next), 
								"next_ind", GINT_TO_POINTER(nextInd));

						// fill in previous line
						int p_ind = 0;
						for( ; p_ind < MAX_PREV_LINE; p_ind++) {
							if( !wpPlans[clientInd].imgPoints[nextInd].line_prev[p_ind] ) {
								wpPlans[clientInd].imgPoints[nextInd].line_prev[p_ind] = wpPlans[clientInd].imgPoints[thisInd].line_next;
								break;
							}
						}
						if( p_ind == MAX_PREV_LINE )
							cout << "FLIGHTPLAN - too many plans converge to one point" << endl;
					}
				}
				j = FindNextWPMaskBitSet(plan_mask, (uint32_t)j);
			}

			// clear these points from the mask so that we don't draw them twice
			InvertAllWPMaskBits(plan_mask);
			WPMaskAND(mask, plan_mask);
		} else
			ClearWPMaskBit(mask, i);

		i = FindNextWPMaskBitSet(mask, (uint32_t)i);
	} // Until the points are gone

	//--  loop through and draw the waypoints points --//
	char pointName[10];
	for( int i = 0; i < NUM_WP_POINTS; i++ ) {
		// get the point
		GPSWaypoint_t gpsPt = wpPlans[clientInd].plan.get_waypoint(i);
		int ind = gpsPt.index;
		if( ind < NUM_WP_POINTS ) {
			// Create group to hold waypoint and text
			wpPlans[clientInd].imgPoints[ind].pointG = goo_canvas_group_new (wpPlans[clientInd].wp_group,
					"x", wpPlans[clientInd].imgPoints[ind].x,
					"y", wpPlans[clientInd].imgPoints[ind].y,
					NULL);
			setup_item(GOO_CANVAS_ITEM(wpPlans[clientInd].imgPoints[ind].pointG));

			// draw circle for orbit point 
			if( gpsPt.OrbitRadius > 0 )
			{
				double delta_pix  = gpsPt.OrbitRadius / gps_map->mPerP( gpsPt.Lat );
				wpPlans[clientInd].imgPoints[ind].orbit = goo_canvas_ellipse_new( 
						wpPlans[clientInd].imgPoints[ind].pointG,
						0.0, 0.0,
						delta_pix, delta_pix,
						"stroke_color", stroke_color.c_str(),
						"line-width", lineWidth,
						NULL );
			}

			// A rectangle for waypoint
			wpPlans[clientInd].imgPoints[ind].point = goo_canvas_rect_new( 
					wpPlans[clientInd].imgPoints[ind].pointG,
					-pointWidth/2.0, -pointWidth/2.0,
					pointWidth, pointWidth,
					"fill_color", "black",
					"stroke_color", "black",
					"line-width", lineWidth,
					NULL) ;

			// text - index number
			sprintf(pointName,"%i",ind);
			wpPlans[clientInd].imgPoints[ind].text = goo_canvas_text_new (
					wpPlans[clientInd].imgPoints[ind].pointG,
					pointName, -1.5*pointWidth, -1.5*pointWidth, -1,
					GTK_ANCHOR_CENTER,
					"fill_color", "blue",
					"font", "Sans 8px",
					NULL);

		}
	}

	//cout << "FPManager::create_canvas_flightplan - bye" << endl;

}

void FPManager::setup_item(GooCanvasItem *item)
{  
	g_signal_connect (G_OBJECT (item), "button-press-event", G_CALLBACK (FPManager::button_press_event_cb), this);
	g_signal_connect (G_OBJECT (item), "button-release-event", G_CALLBACK (FPManager::button_release_event_cb), this);
	g_signal_connect (G_OBJECT (item), "motion-notify-event", G_CALLBACK (FPManager::motion_notify_event_cb), this);
}     

bool FPManager::button_press_event_cb (GooCanvasItem *item, GooCanvasItem *target_item, GdkEventButton *event, FPManager *me)
{
/*
	cout << "FPManager::button_press_event_cb: " 
		<< " me=" << me
		<< " event->type=" << event->type 
		<<"  event->x=" <<  event->x
		<< " event->y=" <<  event->y << endl;
*/

	if( me == NULL )
		return FALSE;

	GdkCursor *pointer;

	// save mouse position, these are world coordinates
	me->mouse_x = event->x;
	me->mouse_y = event->y;

	me->drag_client = -1;
	me->drag_point = -1;

	// find the point for wich this signal came
	for( int clientInd = 0; clientInd < MAX_CLIENTS+1; clientInd++ ) {
		if( me->wpPlans[clientInd].wp_group ) {
			for( int pointInd = 0; pointInd < NUM_WP_POINTS; pointInd++ ) {
				if( me->wpPlans[clientInd].imgPoints[pointInd].pointG == item ) {
					me->drag_client = clientInd;
					me->drag_point = pointInd;

					pointInd = NUM_WP_POINTS;
					clientInd = MAX_CLIENTS+1;
				}
			}
		}
	}

	if( me->drag_client == -1 || me->drag_point == -1 ) {
		cout << " - did not find valid point group for mouse button" << endl;
		return FALSE;
	} 

	switch (event->type) {
		case GDK_2BUTTON_PRESS:
			if( me->creating_plan && me->drag_client == MAX_CLIENTS) {

				int ind = me->numPoints(MAX_CLIENTS);
				GPSWaypoint_t point = me->get_waypoint(MAX_CLIENTS, ind-1);

				if( me->drag_point == ind - 1) {
					// if we double click on final point, make it an orbit
					point.Next           = point.index - 1; // point to previous point
					point.OrbitRadius    = 100; // make 100 m orbit default
					point.OrbitDirection = 0; // make CW orbit direction
				} else {
					// close the flight plan to the point selected
					point.Next = me->drag_point;
				}

				me->set_waypoint(MAX_CLIENTS, point);

				// now that its closed, stop waypoint editing
				me->gps_map->setPlanEditing( FALSE );
				me->clear_client_canvas(MAX_CLIENTS);
				me->create_canvas_flightplan(MAX_CLIENTS);

				// clean up mouse state
				goo_canvas_pointer_ungrab(me->gps_map->getCanvas(), item, event->time);
				me->drag_client = -1;
				me->drag_point  = -1;
				me->drag_plan   = FALSE;
				return TRUE;
			}
			break;

		case GDK_BUTTON_PRESS: 
			switch(event->button) 
			{
				case LEFT_MOUSE:

#ifndef EDIT_REMOTE_FP
					// make sure its the local flight plan when creating
					if( me->drag_client != MAX_CLIENTS ) {
						me->drag_client = -1;
						me->drag_point  = -1;
						me->drag_plan   = FALSE;
						return TRUE;
					}
#endif

					// make sure its the local flight plan when creating
					if( me->creating_plan && me->drag_client != MAX_CLIENTS ) {
						me->drag_client = -1;
						me->drag_point  = -1;
						me->drag_plan   = FALSE;
						return TRUE;
					}

					// set curser based on wether we are moving a point, moving a plan, or rotating a plan
					if(event->state & GDK_CONTROL_MASK)  {
						pointer = gdk_cursor_new(GDK_CIRCLE);
						me->drag_plan   = FALSE;
						me->rotate_plan = FALSE;
					} else if(event->state & GDK_SHIFT_MASK)  {
						pointer = gdk_cursor_new(GDK_CIRCLE);
						me->drag_plan   = FALSE;
						me->rotate_plan = TRUE;
					} else {
						pointer = gdk_cursor_new(GDK_DOTBOX);
						me->rotate_plan = FALSE;
						me->drag_plan   = TRUE;
					}

					// set dragging to true
					me->dragging = TRUE;

					// grab pointG for this point
					goo_canvas_pointer_grab(me->gps_map->getCanvas(), item,
							(GdkEventMask)(GDK_POINTER_MOTION_MASK | GDK_BUTTON_RELEASE_MASK),
							pointer,
							event->time);
					gdk_cursor_destroy(pointer);

					return TRUE;
					break;

				case RIGHT_MOUSE:
					//make sure we are not dragging anything
					me->dragging = FALSE;
					me->create_control_window(event);
					return TRUE;
					break;

				case MIDDLE_MOUSE:
					//make sure we are not dragging anything
					me->dragging = FALSE;
					break;

				default:
					printf("Unknown mouse button value\n");
					break;
			} 
		default:
			break;
	}

	// Returning FALSE propagates the event to parent items;
	// returning TRUE ends event propagation. 
	return FALSE;
}

bool FPManager::button_release_event_cb(GooCanvasItem *item, GooCanvasItem *target_item, GdkEventButton *event, FPManager *me)
{
	//cout << "FPManager::button_release_event_cb" << endl;
	if( me == NULL )
		return FALSE;

	if( me->drag_client != -1 || me->drag_point != -1 ) {
		goo_canvas_pointer_ungrab(me->gps_map->getCanvas(), item, event->time);

#ifdef EDIT_REMOTE_FP
		// need to send the command if we edited a clients flight plan
		if( me->drag_client < MAX_CLIENTS ) {
			if( !me->send_flightplan(me->drag_client, me->drag_client, me->drag_point) ) {
				cout << "Did not send flight plan" << endl;
			}
		}
#endif
	}

	me->drag_client = -1;
	me->drag_point  = -1;
	me->dragging    = FALSE;
	me->rotate_plan = FALSE;
	me->last_theta  = NAN;

	// Returning FALSE propagates the event to parent items;
	// returning TRUE ends event propagation. 
	return FALSE;
}

bool FPManager::motion_notify_event_cb(GooCanvasItem *item, GooCanvasItem *target_item, GdkEventMotion *event, FPManager *me)
{
	//cout << "FPManager::motion_notify_event_cb" << endl;
	if( me == NULL )
		return FALSE;

	if( me->dragging == TRUE && me->drag_client >= 0 && me->drag_point >= 0) 
	{
		if( !me->drag_plan && !me->rotate_plan ) {
			me->translate_point(me->drag_client, me->drag_point,
					event->x - me->mouse_x, event->y - me->mouse_y);

		} else {
			//-- with starting index, get mask for plan and check existence
			WPMask_t Mask;
			int numPoints = me->wpPlans[me->drag_client].plan.GetPointsInPlan(Mask, me->drag_point);
			if( numPoints <= 0 ) {
				cout << "!!! no points in waypoint plan" << endl;
				return false;
			}

			// if rotating a plan, get the angle to rotate
			double dTheta = 0;
			if( me->rotate_plan ) {
				// get the x,y img coordinate of the selected point
				double base_x, base_y;
				GooCanvasItem *baseG = me->wpPlans[me->drag_client].imgPoints[me->drag_point].pointG;
				g_object_get (G_OBJECT (baseG), "x", &base_x, "y", &base_y, NULL);

				// calc angle
				double theta = atan2( event->y - base_y, event->x - base_x);
				if( me->last_theta != me->last_theta ) me->last_theta = 0;

				// calc delta angle
				dTheta = theta - me->last_theta;

				// save value for next round
				me->last_theta = theta;
			}

			// for a flight plan, we need to find the first point, then loop through
			// the plan to update the points
			int curr_ind = me->wpPlans[me->drag_client].plan.find_first_in_plan(me->drag_point);

			//-- now loop through all points in plan
			while( GetWPMaskBit(Mask, curr_ind) )
			{
				if( me->rotate_plan ) {
					// don't rotate point we are rotating about
					if( curr_ind != me->drag_point ){
						me->rotate_point(me->drag_client, curr_ind, dTheta);
					}
				} else
					me->translate_point(me->drag_client, curr_ind,
							event->x - me->mouse_x, event->y - me->mouse_y);	

				// clear this point from the mask, and get the next
				ClearWPMaskBit(Mask, curr_ind);
				curr_ind = me->wpPlans[me->drag_client].plan.Plan[curr_ind].Next;
			}
		}

		// don't forget to save the mouse location
		me->mouse_x = event->x;
		me->mouse_y = event->y;

#ifdef EDIT_REMOTE_FP
		if(me->drag_client != MAX_CLIENTS)
			me->setModified(me->drag_client, me->drag_point);
#endif
	}

	// Returning FALSE propagates the event to parent items;
	// returning TRUE ends event propagation. 
	return FALSE;
}

void FPManager::rotate_point(int clientInd, int pointInd, double dTheta)
{
	double item_x,item_y;
	double new_lon,new_lat;
	//cout << "FPManager::rotate_point - pointInd=" << pointInd << endl;

	// don't do any work if we don't have to
	if( dTheta == 0 ) return;

	// get the x,y img coordinate of the selected point
	double base_x, base_y;
	GooCanvasItem *baseG = wpPlans[drag_client].imgPoints[drag_point].pointG;
	g_object_get (G_OBJECT (baseG), "x", &base_x, "y", &base_y, NULL);

	// get the x,y img coordinate of the current pointG
	GooCanvasItem *pointG = wpPlans[clientInd].imgPoints[pointInd].pointG;
	g_object_get (G_OBJECT (pointG), "x", &item_x, "y", &item_y, NULL);

	double dX = item_x - base_x;
	double dY = item_y - base_y;

	item_x = cos(dTheta) * dX - sin(dTheta) * dY + base_x;
	item_y = sin(dTheta) * dX + cos(dTheta) * dY + base_y;

	// convert the new location to lat, lon
	new_lon = item_x;
	new_lat = item_y;
	gps_map->layerToGeo(GPSMap::FP_LAYER, new_lon, new_lat);

	// save the coordinates
	GPSWaypoint_t point = wpPlans[clientInd].plan.get_waypoint(pointInd);
	point.Lat = new_lat;
	point.Lon = new_lon;
	wpPlans[clientInd].plan.set_waypoint(point);

	// now update the canvas point and lines
	g_object_set (G_OBJECT (pointG), "x", item_x, "y", item_y, NULL);

	GooCanvasPoints *points;
	if( wpPlans[clientInd].imgPoints[pointInd].line_next ) {
		g_object_get (wpPlans[clientInd].imgPoints[pointInd].line_next, "points", &points, NULL);
		points->coords[0] = item_x;
		points->coords[1] = item_y;
		g_object_set (wpPlans[clientInd].imgPoints[pointInd].line_next, "points", points, NULL);
		goo_canvas_points_unref(points);
	}

	for( int p_ind = 0; p_ind < MAX_PREV_LINE; p_ind++) {
		if( wpPlans[clientInd].imgPoints[pointInd].line_prev[p_ind] ) {
			g_object_get (wpPlans[clientInd].imgPoints[pointInd].line_prev[p_ind], "points", &points, NULL);
			points->coords[2] = item_x;
			points->coords[3] = item_y;
			g_object_set (wpPlans[clientInd].imgPoints[pointInd].line_prev[p_ind], "points", points, NULL);
			goo_canvas_points_unref(points);
		}
	}
}


void FPManager::translate_point(int clientInd, int pointInd, double dX, double dY)
{
	double item_x,item_y;
	double new_lon,new_lat;
	//cout << "FPManager::translate_point" << endl;
	
	GooCanvasItem *pointG = wpPlans[clientInd].imgPoints[pointInd].pointG;

	// get the x,y img coordinate of the pointG
	g_object_get (G_OBJECT (pointG), "x", &item_x, "y", &item_y, NULL);

	// now translate them by mouse motion
	item_x += dX;
	item_y += dY;

	// convert the new location to lat, lon
	new_lon = item_x;
	new_lat = item_y;
	gps_map->layerToGeo(GPSMap::FP_LAYER, new_lon, new_lat);

	// save the coordinates
	GPSWaypoint_t point = wpPlans[clientInd].plan.get_waypoint(pointInd);
	point.Lat = new_lat;
	point.Lon = new_lon;
	wpPlans[clientInd].plan.set_waypoint(point);

	// now update the canvas point and lines
	g_object_set (G_OBJECT (pointG), "x", item_x, "y", item_y, NULL);

	GooCanvasPoints *points;
	if( wpPlans[clientInd].imgPoints[pointInd].line_next ) {
		g_object_get (wpPlans[clientInd].imgPoints[pointInd].line_next, "points", &points, NULL);
		points->coords[0] = item_x;
		points->coords[1] = item_y;
		g_object_set (wpPlans[clientInd].imgPoints[pointInd].line_next, "points", points, NULL);
		goo_canvas_points_unref(points);
	}

	for( int p_ind = 0; p_ind < MAX_PREV_LINE; p_ind++) {
		if( wpPlans[clientInd].imgPoints[pointInd].line_prev[p_ind] ) {
			g_object_get (wpPlans[clientInd].imgPoints[pointInd].line_prev[p_ind], "points", &points, NULL);
			points->coords[2] = item_x;
			points->coords[3] = item_y;
			g_object_set (wpPlans[clientInd].imgPoints[pointInd].line_prev[p_ind], "points", points, NULL);
			goo_canvas_points_unref(points);
		}
	}
}

void FPManager::clear_client(int clientInd)
{
	//cout << "FPManager::clear_client(int clientInd)" << endl;

	clear_client_canvas(clientInd);
	wpPlans[clientInd].plan.clear_list();
}

void FPManager::clear_client_canvas(int clientInd)
{
	//cout << "FPManager::clear_client_canvas" << endl;

	for( int i=0; i < NUM_WP_POINTS; i++ ) {

		// by removing all next lines, we are removing all previous line items
		if( GOO_IS_CANVAS_ITEM(wpPlans[clientInd].imgPoints[i].line_next ) )
			goo_canvas_item_remove(wpPlans[clientInd].imgPoints[i].line_next);
		wpPlans[clientInd].imgPoints[i].line_next = NULL;

		// by removing the point group, we are removing the text, orbit, and point items
		// as well.
		if( GOO_IS_CANVAS_ITEM(wpPlans[clientInd].imgPoints[i].pointG ) )
			goo_canvas_item_remove(wpPlans[clientInd].imgPoints[i].pointG);
		wpPlans[clientInd].imgPoints[i].pointG = NULL;

		wpPlans[clientInd].imgPoints[i].point = NULL;
		wpPlans[clientInd].imgPoints[i].text  = NULL;
		wpPlans[clientInd].imgPoints[i].orbit = NULL;
		for( int p_ind = 0; p_ind < MAX_PREV_LINE; p_ind++ ) {
			wpPlans[clientInd].imgPoints[i].line_prev[p_ind] = NULL;
		}

	}
}


int FPManager::create_control_window( GdkEventButton *event)
{
	// Get the popup menu
	if( !menu )
		menu = gtk_ui_manager_get_widget (menu_manager, "/FPPopup");
	gtk_menu_popup (GTK_MENU(menu), NULL, NULL, NULL, NULL, event->button, event->time);

	return TRUE;
}

int FPManager::edit_fp_cb(GtkAction *action, FPManager *me)
{
	if( me == NULL )
		return FALSE;

#ifndef EDIT_REMOTE_FP
	if( me->drag_client < MAX_CLIENTS ) {
		cout << " !!! can't edit a clients waypoint" << endl;
		return true;
	}
#endif

	me->create_fpeditor_window();

	// Returning FALSE propagates the event to parent items;
	// returning TRUE ends event propagation. 
	return true;
}

int FPManager::copy_fp_cb(GtkAction *action, FPManager *me)
{
	if( me == NULL )
		return FALSE;

	if( me->drag_client == MAX_CLIENTS ) {
		cout << " !!! can't copy the local flightplan, its already local" << endl;
		// returning TRUE ends event propagation. 
		return true;
	}
	
	// if there are any local flight plan points, simply clear all of them 
	// before we copy
	if( me->wpPlans[ MAX_CLIENTS ].plan.numPoints() > 0 ) {
		me->wpPlans[MAX_CLIENTS].plan.clear_list();
		me->clear_client_canvas(MAX_CLIENTS);
	}

	// for a flight plan, we need to find the first point, then loop through
	// the plan to copy the plan
	int pointInd = me->wpPlans[me->drag_client].plan.find_first_in_plan(me->drag_point);
	if( pointInd < 0 ) {
		printf("Waypoint plan does not exists\n");
		// returning TRUE ends event propagation. 
		return true;
	}

	//-- with starting index, get mask for plan and check existence
	WPMask_t Mask;
	int numPoints = me->wpPlans[me->drag_client].plan.GetPointsInPlan(Mask, pointInd);
	if( numPoints <= 0 ) {
		cout << "!!! no points in waypoint plan" << endl;
		// returning TRUE ends event propagation. 
		return true;
	}

	//-- now loop through all points in plan
	while( GetWPMaskBit(Mask, pointInd) )
	{
		GPSWaypoint_t pt = me->get_waypoint(me->drag_client, pointInd);
		// get the current point
		if( !me->set_waypoint(MAX_CLIENTS, pt ) )
			cout << "!! Could not copy waypoint from client to local" << endl;

		// clear this point from the mask, and get the next
		ClearWPMaskBit(Mask, pointInd);
		pointInd = pt.Next;
	}

	// now draw the new plan
	me->create_canvas_flightplan(MAX_CLIENTS);

	// Returning FALSE propagates the event to parent items;
	// returning TRUE ends event propagation. 
	return true;
}

int FPManager::send_fp_cb(GtkAction *action, FPManager *me)
{
	//cout << "FPManager::send_fp_cb" << endl;

	if( me == NULL || me->drag_client == -1 )
		return FALSE;

	// if a clients flightplan, simply send the edited one
	if( me->drag_client < MAX_CLIENTS ) {
		me->send_flightplan( me->drag_client, me->drag_client, me->drag_point);
		return true;
	}

	// otherwise, we need a pop up window to finish getting values such as altitude
	// FIXME - add popup
	me->create_fpsend_window();

	// Returning FALSE propagates the event to parent items;
	// returning TRUE ends event propagation. 
	return true;
}

int FPManager::save_fp_cb(GtkAction *action, FPManager *me)
{
	if( me == NULL )
		return FALSE;

	//cout << "FPManager::save_fp_cb - callback_action=" << action << endl;

	GtkWidget *dialog;
	char *filename = NULL;

	dialog = gtk_file_chooser_dialog_new ("Save Flight Plan ...",
			NULL,
			GTK_FILE_CHOOSER_ACTION_SAVE,
			GTK_STOCK_CANCEL, GTK_RESPONSE_CANCEL,
			GTK_STOCK_SAVE, GTK_RESPONSE_ACCEPT,
			NULL);

	String lastDir = me->gps_map->get_last_directory();
	if ( lastDir != "")
		gtk_file_chooser_set_current_folder (GTK_FILE_CHOOSER (dialog), lastDir.c_str());

	GtkFileFilter *filter = gtk_file_filter_new ();
	gtk_file_filter_add_pattern (filter, "*" FLIGHTPLAN_EXT_STRING);
	gtk_file_chooser_set_filter( GTK_FILE_CHOOSER (dialog), filter);

	if (gtk_dialog_run (GTK_DIALOG (dialog)) == GTK_RESPONSE_ACCEPT) {
		filename = gtk_file_chooser_get_filename (GTK_FILE_CHOOSER (dialog));
	}

	gtk_object_destroy (GTK_OBJECT(filter));
	gtk_widget_destroy (dialog);

	if( filename ) {
		me->gps_map->set_last_directory_from_filename(filename);
		me->save_flightplan(me->drag_client, me->drag_point, filename);
		g_free (filename);
	}

	// Returning FALSE propagates the event to parent items;
	// returning TRUE ends event propagation. 
	return true;
}

int FPManager::clear_fp_cb(GtkAction *action, FPManager *me)
{
	if( me == NULL )
		return FALSE;

	if( me->drag_client < MAX_CLIENTS ) {
		cout << " !!! can't clear a clients flight plan" << endl;
		return false;
	}

	me->clear_client_canvas(me->drag_client);
	me->wpPlans[ me->drag_client ].plan.clear_plan(me->drag_point);
	me->create_canvas_flightplan(me->drag_client);

	// Returning FALSE propagates the event to parent items;
	// returning TRUE ends event propagation. 
	return true;
}

/* columns for GtkTree */
enum
{
	FPS_IND_COLUMN = 0,
	FPS_NAME_COLUMN,
	FPS_NUM_COLUMNS
};

GtkTreeView* FPManager::create_fps_tree(GtkTreeView *tree, GtkWidget *sw)
{
	// create tree store model
	if(!fps_list_store) {
		fps_list_store = gtk_list_store_new ( FPS_NUM_COLUMNS,
	                                  G_TYPE_INT,     // link ind
	                                  G_TYPE_STRING  // name
	                                );
	}

	// draw the view of the tree
	if( !tree ) tree = format_fps_treeview();

	// add to container
	gtk_container_add( GTK_CONTAINER (sw), GTK_WIDGET(tree) );
	gtk_widget_show( GTK_WIDGET(tree) );

	return tree;
}
	

GtkTreeView * FPManager::format_fps_treeview()
{
	gint col_offset;
	GtkCellRenderer *renderer;

	// create tree view 
	GtkTreeView *tree = GTK_TREE_VIEW(gtk_tree_view_new_with_model (GTK_TREE_MODEL (fps_list_store)));

	//cout << "FPManager::format_fps_treeview - tree=" << tree << endl;

	// setup tree functionality
	gtk_tree_view_set_rules_hint(GTK_TREE_VIEW(tree), TRUE);
	gtk_tree_view_set_grid_lines(GTK_TREE_VIEW(tree), GTK_TREE_VIEW_GRID_LINES_BOTH);
	gtk_tree_view_set_headers_visible(GTK_TREE_VIEW(tree),FALSE);
	gtk_tree_view_set_headers_clickable(GTK_TREE_VIEW(tree), FALSE);
	gtk_tree_view_set_reorderable(GTK_TREE_VIEW(tree), TRUE);
	gtk_tree_view_set_enable_search(GTK_TREE_VIEW(tree), FALSE);

	GtkTreeSelection *select;
	select = gtk_tree_view_get_selection (GTK_TREE_VIEW (tree));
	gtk_tree_selection_set_mode (select, GTK_SELECTION_SINGLE);

	// column for node names 
	renderer = gtk_cell_renderer_text_new ();
	g_object_set (renderer, "xalign", 0.0, NULL);
	col_offset = gtk_tree_view_insert_column_with_attributes (GTK_TREE_VIEW (tree),
			-1, "Node", renderer, 
			"text", FPS_NAME_COLUMN,
			NULL);

	return tree;
}


void FPManager::create_fpsend_window()
{
	//cout << "FPManager::create_fpsend_window()" << endl;

	if( guiStr.builder == NULL ) {
		cout << "create_fpsend_window() - no builder for gui" << endl;
		return;
	}

	GtkWidget *apply_button  = GTK_WIDGET (gtk_builder_get_object (guiStr.builder, "fp_send_ok"));
	GtkWidget *cancel_button = GTK_WIDGET (gtk_builder_get_object (guiStr.builder, "fp_send_cancel"));

	// create window and setup signals
	if( !fp_send ) {
		fp_send = GTK_WIDGET (gtk_builder_get_object (guiStr.builder, "fpsend_dialog"));
		fps_sw        = GTK_WIDGET (gtk_builder_get_object (guiStr.builder, "send_client_vsw"));
		fps_alt       = GTK_WIDGET (gtk_builder_get_object (guiStr.builder, "fps_plan_alt"));
		fps_index     = GTK_WIDGET (gtk_builder_get_object (guiStr.builder, "fps_plan_index"));

		if( !fp_send || !apply_button || !cancel_button || !fps_sw){
			cout << "Invalid xml ui file for FP send dialog" << endl;
			return;
		}

		// callbacks / signals
		g_signal_connect (GTK_OBJECT(fp_send), "delete_event", G_CALLBACK(FPManager::fpsend_window_destroy), fp_editor);
		g_signal_connect (GTK_OBJECT(apply_button), "clicked", G_CALLBACK(FPManager::fpsend_button_clicked), this);
		g_signal_connect (G_OBJECT(cancel_button), "clicked", G_CALLBACK(FPManager::fpsend_button_clicked), this);
		g_object_set_data(G_OBJECT(apply_button), "label", (gpointer)"apply");
		g_object_set_data(G_OBJECT(cancel_button), "label", (gpointer)"cancel");

		// create list view 
		fps_treeview = create_fps_tree(fps_treeview, fps_sw);
	}

	// make sure the input boxes are set back to default values
	gtk_spin_button_set_value(GTK_SPIN_BUTTON(fps_alt), -1.0);
	gtk_spin_button_set_value(GTK_SPIN_BUTTON(fps_index), -1.0);

	// update the list with all clients
	update_client_list();

	gtk_widget_show_all (fp_send);
}

// If you return FALSE in the "delete_event" signal handler, GTK will emit
// the "destroy" signal. Returning TRUE means you don't want the window to be
// destroyed.  
gboolean FPManager::fpsend_window_destroy(GtkWidget * widget, GdkEventKey * event, gpointer user_data)
{
	if( user_data ) {
		gtk_widget_hide_all (GTK_WIDGET(user_data));
		return TRUE;
	}
	return FALSE;
}

void FPManager::fpsend_button_clicked(GtkButton *button, gpointer user_data)
{
	char * label = (char *)g_object_get_data(G_OBJECT(button), "label");
	//printf( " -- fpsend_button_clicked : %s -- \n", label);

	FPManager *me = (FPManager *)user_data;
	if( !me ) return;

	// hide the window, it is not destroyed
	gtk_widget_hide(me->fp_send);

	// if not the apply button, simply return
	if( strcmp(label,"apply") != 0 ) return;

	// get the altitude
	float alt = gtk_spin_button_get_value(GTK_SPIN_BUTTON(me->fps_alt));
	
	// get the index
	int index = (int)gtk_spin_button_get_value(GTK_SPIN_BUTTON(me->fps_index));

	GtkTreeIter  iter;
	GtkTreeSelection *selection = gtk_tree_view_get_selection(me->fps_treeview);

	GtkTreeModel *model = GTK_TREE_MODEL(me->fps_list_store);
	if( gtk_tree_selection_get_selected(selection, &model, &iter) ) {
		// get data from current row
		int c_ind = -1;
		gtk_tree_model_get (GTK_TREE_MODEL(me->fps_list_store), &iter, FPS_IND_COLUMN, &c_ind, -1);

		//cout << "Client=" << c_ind << " alt=" << alt << " index=" << index << endl;

		me->send_flightplan( MAX_CLIENTS, c_ind, me->drag_point, index, alt);
	}

}

//-- Flight plan editor
void FPManager::create_fpeditor_window()
{
	//cout << "FPManager::create_fpeditor_window()" << endl;

	if( guiStr.builder == NULL ) {
		cout << "create_param_window() - no builder for gui" << endl;
		return;
	}

	GtkWidget *sw            = GTK_WIDGET (gtk_builder_get_object (guiStr.builder, "wp_sw"));
	GtkWidget *apply_button  = GTK_WIDGET (gtk_builder_get_object (guiStr.builder, "wp_ok_button"));
	GtkWidget *cancel_button = GTK_WIDGET (gtk_builder_get_object (guiStr.builder, "wp_cancel_button"));

	// create window and setup signals
	if( !fp_editor ) {
		fp_editor = GTK_WIDGET (gtk_builder_get_object (guiStr.builder, "fpeditor_dialog"));

		// callbacks / signals
		g_signal_connect (GTK_OBJECT(fp_editor), "delete_event", G_CALLBACK(FPManager::fpeditor_window_destroy), fp_editor);
		g_signal_connect (GTK_OBJECT(apply_button), "clicked", G_CALLBACK(FPManager::fpeditor_button_clicked), this);
		g_signal_connect (G_OBJECT(cancel_button), "clicked", G_CALLBACK(FPManager::fpeditor_button_clicked), this);
		g_object_set_data(G_OBJECT(apply_button), "label", (gpointer)"apply");
		g_object_set_data(G_OBJECT(cancel_button), "label", (gpointer)"cancel");
	}

	if( !fp_editor || !apply_button || !cancel_button || !sw ){
		cout << "Invalid xml ui file for fpeditor" << endl;
		return;
	}

   // clear the contents of the container
	GList* childList = gtk_container_get_children(GTK_CONTAINER (sw));
	if( childList ) {
		gtk_widget_destroy(GTK_WIDGET(g_list_nth_data(childList, 0) ) );
	}

	// cleanup previous model
	if( list_model ) g_object_unref (list_model);
	list_model=NULL;

	// create list view 
	create_model_and_view();

	// create model 
	fill_model();
	gtk_tree_view_set_model (GTK_TREE_VIEW (list_view), list_model);
	
	// setup list functionality
	gtk_tree_view_set_rules_hint (GTK_TREE_VIEW (list_view), TRUE);

	gtk_container_add (GTK_CONTAINER (sw), list_view);

	gtk_widget_show_all (fp_editor);
}

void FPManager::create_model_and_view(void)
{
	GtkCellRenderer *renderer;

	// create list store, i.e. the model
	list_store = gtk_list_store_new (NUM_COLUMNS, 
	                            G_TYPE_UCHAR, // index
	                            G_TYPE_UCHAR, // next
	                            G_TYPE_FLOAT, // lat
	                            G_TYPE_FLOAT, // lon
	                            G_TYPE_FLOAT, // alt
	                            //G_TYPE_FLOAT, // wind interval
										 //G_TYPE_BOOLEAN, // preturn
										 //G_TYPE_BOOLEAN, // lights
										 //G_TYPE_BOOLEAN, // slope
										 //G_TYPE_BOOLEAN, // land
										 //G_TYPE_BOOLEAN, // parachute
										 //G_TYPE_BOOLEAN, // drop
										 //G_TYPE_BOOLEAN, // camera
										 //G_TYPE_BOOLEAN, // hover
										 G_TYPE_BOOLEAN, // orbit dir
										 G_TYPE_FLOAT // orbit radius
										 //G_TYPE_FLOAT, // orbit time
										 //G_TYPE_FLOAT, // orbit above
										 //G_TYPE_FLOAT, // orbit below
										 //G_TYPE_INT // user
										 );

	list_model = GTK_TREE_MODEL (list_store);

	//-- create list view
	list_view = gtk_tree_view_new ();

	// loop through all of the different columns
	for( int i=0; i < NUM_COLUMNS; i++) {
		// if its a boolean, give it a check box
		if( wp_entries[i].type == G_TYPE_BOOLEAN ) {
			renderer = gtk_cell_renderer_toggle_new ();

#ifndef EDIT_REMOTE_FP
			if( drag_client == MAX_CLIENTS ) 
#endif
				g_signal_connect(renderer, "toggled", (GCallback) cell_toggled_callback, list_model);

			gtk_tree_view_insert_column_with_attributes (GTK_TREE_VIEW (list_view),
					-1, wp_entries[i].name, renderer,
					"active", wp_entries[i].col,
					NULL);
		// otherwise draw and give editable text
		} else {
			renderer = gtk_cell_renderer_text_new ();
#ifndef EDIT_REMOTE_FP
			if( drag_client == MAX_CLIENTS ) 
#endif
			{
				g_object_set(renderer, "editable", TRUE, NULL);
				g_signal_connect(renderer, "edited", (GCallback) cell_edited_callback, list_model);
			}

			gtk_tree_view_insert_column_with_attributes (GTK_TREE_VIEW (list_view),
					-1, wp_entries[i].name, renderer,
					"text", wp_entries[i].col,
					NULL);
		}

		g_object_set_data(G_OBJECT(renderer), "my_column_num", GUINT_TO_POINTER(wp_entries[i].col));
	}
}

void FPManager::fill_model (void)
{
	GtkTreeIter    iter;

	// for a flight plan, we need to find the first point, then loop through
	// the plan to update the points
	int pointInd = wpPlans[drag_client].plan.find_first_in_plan(drag_point);
	if( pointInd < 0 ) {
		printf("Waypoint plan does not exists\n");
		return;
	}

	//-- with starting index, get mask for plan and check existence
	WPMask_t Mask;
	int numPoints = wpPlans[drag_client].plan.GetPointsInPlan(Mask, pointInd);
	if( numPoints <= 0 ) {
		cout << "!!! no points in waypoint plan" << endl;
		return;
	}

	//-- now loop through all points in plan
	while( GetWPMaskBit(Mask, pointInd) )
	{
		// get the current point
		GPSWaypoint_t gpsPt = wpPlans[drag_client].plan.get_waypoint(pointInd);

		// append a row
		gtk_list_store_append (list_store, &iter);

		// fill in some data
		gtk_list_store_set (list_store, &iter,
				COL_INDEX, gpsPt.index,
				COL_NEXT, gpsPt.Next,
				COL_LAT, gpsPt.Lat,
				COL_LON, gpsPt.Lon,
				COL_ALT, gpsPt.Alt,
				//COL_WINDINTERVAL, gpsPt.WindInterval,
				//COL_PRETURN, gpsPt.Preturn,
				//COL_LIGHTS, gpsPt.Lights,
				//COL_SLOPE, gpsPt.Slope,
				//COL_LAND, gpsPt.Land,
				//COL_PARACHUTE, gpsPt.Parachute,
				//COL_DROP, gpsPt.Drop,
				//COL_CAMERA, gpsPt.Camera,
				//COL_HOVER, gpsPt.Hover,
				COL_ORBITDIRECTION, gpsPt.OrbitDirection,
				COL_ORBITRADIUS, gpsPt.OrbitRadius,
				//COL_ORBITTIME, gpsPt.OrbitTime,
				//COL_ORBITABOVEALT, gpsPt.OrbitAboveAlt,
				//COL_ORBITBELOWALT, gpsPt.OrbitBelowAlt,
				//COL_USER, gpsPt.User,
				-1);


		// clear this point from the mask, and get the next
		ClearWPMaskBit(Mask, pointInd);
		pointInd = wpPlans[drag_client].plan.Plan[pointInd].Next;
	}

}

void FPManager::cell_toggled_callback (GtkCellRendererToggle *cell, gchar *path_string, gpointer user_data)
{
	//cout << "FPManager::cell_toggled_callback" << endl;

	GtkTreeModel *model = (GtkTreeModel *)user_data;
	GtkTreePath *path   = gtk_tree_path_new_from_string (path_string);
	guint col = GPOINTER_TO_UINT(g_object_get_data(G_OBJECT(cell), "my_column_num"));
	GtkTreeIter iter;
	gboolean toggle_item;

	// get toggled iter 
	gtk_tree_model_get_iter (model, &iter, path);
	gtk_tree_model_get (model, &iter, col, &toggle_item, -1);

	// do something with the value 
	toggle_item ^= 1;

	// set new value 
	gtk_list_store_set (GTK_LIST_STORE (model), &iter, col, toggle_item, -1);
}

void FPManager::cell_edited_callback (GtkCellRendererText *cell, gchar *path_string, gchar *new_text, gpointer user_data)
{
	//cout << "FPManager::cell_edited_callback - new_text=" << new_text << endl;

	GtkTreeModel *model = (GtkTreeModel *)user_data;
	GtkTreePath *path   = gtk_tree_path_new_from_string (path_string);
	guint col           = GPOINTER_TO_UINT(g_object_get_data(G_OBJECT(cell), "my_column_num"));

	// get toggled iter 
	GtkTreeIter iter;
	gtk_tree_model_get_iter (model, &iter, path);

	for( size_t i=0; i<strlen(new_text); i++) {
		if( (new_text[i] < '0' || new_text[i] > '9') && (new_text[i] != '-' && new_text[i] != '.')) {
			cout << BLINKING YELLOW << "WARNING: dumb user at console - only skilled people should run this program" << ATTROFF << endl;
			return;
		}
	}

	switch(col) {
		case COL_INDEX: case COL_NEXT: {
			errno = 0;
			long int val = strtol(new_text, NULL, 10);
			if( errno == 0 && val >= 0 && val <= 255 ) {
				gtk_list_store_set (GTK_LIST_STORE (model), &iter, col, (guchar)val, -1);
			} else {
				cout << BLINKING YELLOW << "WARNING: dumb user at console - only skilled people should run this program" << ATTROFF << endl;
			}
			break;
		}
/*
		case COL_USER: {
			errno = 0;
			long int val = strtol(new_text, NULL, 10);
			if( errno == 0) {
				gtk_list_store_set (GTK_LIST_STORE (model), &iter, col, (gint)val, -1);
			} else {
				cout << BLINKING YELLOW << "WARNING: dumb user at console - only skilled people should run this program" << ATTROFF << endl;
			}
			break;
		}
*/
		default: {
			errno = 0;
			float val = strtof(new_text, NULL);
			if( errno == 0) {
				gtk_list_store_set (GTK_LIST_STORE (model), &iter, col, (gfloat)val, -1);
			} else {
				cout << BLINKING YELLOW << "WARNING: dumb user at console - only skilled people should run this program" << ATTROFF << endl;
			}
			break;
		}
	}
}


// If you return FALSE in the "delete_event" signal handler, GTK will emit
// the "destroy" signal. Returning TRUE means you don't want the window to be
// destroyed.  
gboolean FPManager::fpeditor_window_destroy(GtkWidget * widget, GdkEventKey * event, gpointer user_data)
{
	if( user_data ) {
		gtk_widget_hide_all (GTK_WIDGET(user_data));
		return TRUE;
	}
	return FALSE;
}

void FPManager::fpeditor_button_clicked(GtkButton *button, gpointer user_data)
{
	char * label = (char *)g_object_get_data(G_OBJECT(button), "label");
	//printf( " -- fpeditor_button_clicked : %s-- \n", label);

	FPManager *me = (FPManager *)user_data;
	if( !me ) return;
	bool newPt = false;
	uint8_t minInd = 255;

	// hide the window, it is not destroyed
	gtk_widget_hide(me->fp_editor);

	// now process the values
	if( strcmp(label,"apply") != 0 ) return;

	GtkTreeIter  iter;
	gboolean     valid;
	valid = gtk_tree_model_get_iter_first (GTK_TREE_MODEL (me->list_model), &iter);

	while (valid) {
		// get the current point
		GPSWaypoint_t gpsPt;

		// get data from current row
		gtk_tree_model_get (me->list_model, &iter,
				COL_INDEX, &gpsPt.index,
				COL_NEXT, &gpsPt.Next,
				COL_LAT, &gpsPt.Lat,
				COL_LON, &gpsPt.Lon,
				COL_ALT, &gpsPt.Alt,
				//COL_WINDINTERVAL, &gpsPt.WindInterval,
				//COL_PRETURN, &gpsPt.Preturn,
				//COL_LIGHTS, &gpsPt.Lights,
				//COL_SLOPE, &gpsPt.Slope,
				//COL_LAND, &gpsPt.Land,
				//COL_PARACHUTE, &gpsPt.Parachute,
				//COL_DROP, &gpsPt.Drop,
				//COL_CAMERA, &gpsPt.Camera,
				//COL_HOVER, &gpsPt.Hover,
				COL_ORBITDIRECTION, &gpsPt.OrbitDirection,
				COL_ORBITRADIUS, &gpsPt.OrbitRadius,
				//COL_ORBITTIME, &gpsPt.OrbitTime,
				//COL_ORBITABOVEALT, &gpsPt.OrbitAboveAlt,
				//COL_ORBITBELOWALT, &gpsPt.OrbitBelowAlt,
				//COL_USER, &gpsPt.User,
				-1);
		gpsPt.validElements = 20;

		// some debuging output
		//cout << gpsPt << endl;

		// track a min index for later use
		if( gpsPt.index < minInd ) minInd = gpsPt.index;

		// set the point
		if( ! me->wpPlans[me->drag_client].plan.set_waypoint(gpsPt) ) printf("!!==== did not set edited point\n");
		else newPt = true;

		// next tree item
		valid = gtk_tree_model_iter_next (GTK_TREE_MODEL (me->list_model), &iter);
	}

	if( newPt && minInd < 255) {
		// first clear any load flight plan items for this client
		me->clear_client_canvas(me->drag_client);

		// draw the new, edited plan
		me->create_canvas_flightplan(me->drag_client);

#ifdef EDIT_REMOTE_FP
		if(me->drag_client != MAX_CLIENTS)
			me->setModified(me->drag_client, me->drag_point);
#endif

		if( !me->wpPlans[me->drag_client].plan.CheckFlightPlanClosure( minInd ) ) {
			printf("!!===== CMD DOES NOT HAVE A CLOSING LOOP\n");
		} //else cout << " [done] " << endl;
	}

#ifdef EDIT_REMOTE_FP
	// need to send the command if we edited a clients flight plan
	if( me->drag_client < MAX_CLIENTS ) {
		if( !me->send_flightplan(me->drag_client, me->drag_client, me->drag_point) ) {
			cout << "Did not send flight plan to client" << endl;
		}
	}
#endif

}

void FPManager::create_default_to_plan()
{
	if( guiStr.builder == NULL ) {
		cout << "create_default_to_plan() - no builder for gui" << endl;
		return;
	}

	// create window and setup signals
	if( !to_win ) {
		to_win    = GTK_WIDGET (gtk_builder_get_object (guiStr.builder, "takeoff_dialog"));
		to_index  = GTK_WIDGET (gtk_builder_get_object (guiStr.builder, "to_index"));
		to_width  = GTK_WIDGET (gtk_builder_get_object (guiStr.builder, "to_width"));
		to_height = GTK_WIDGET (gtk_builder_get_object (guiStr.builder, "to_height"));
		to_alt    = GTK_WIDGET (gtk_builder_get_object (guiStr.builder, "to_alt"));
		to_sw     = GTK_WIDGET (gtk_builder_get_object (guiStr.builder, "to_client_vsw"));

		if( !to_win || !to_index || !to_width || !to_height || !to_alt || !to_sw) {
			cout << "Invalid xml ui file for default take-off dialog" << endl;
			return;
		}
		// add the dialog response buttons
		gtk_dialog_add_button(GTK_DIALOG(to_win), GTK_STOCK_CANCEL, 1);
		gtk_widget_grab_focus( gtk_dialog_add_button(GTK_DIALOG(to_win), GTK_STOCK_OK, 2) );

		// create list view 
		to_treeview = create_fps_tree(to_treeview, to_sw);
	}

	// update the list with all clients
	update_client_list();

	int retval = gtk_dialog_run(GTK_DIALOG(to_win)); 
	gtk_widget_hide(GTK_WIDGET(to_win));

	if( retval != 2 ) {
		return;
	}

	// get values from dialog
	int index    = (int)gtk_spin_button_get_value(GTK_SPIN_BUTTON(to_index));
	float height = gtk_spin_button_get_value(GTK_SPIN_BUTTON(to_height));
	float width  = gtk_spin_button_get_value(GTK_SPIN_BUTTON(to_width));
	float alt    = gtk_spin_button_get_value(GTK_SPIN_BUTTON(to_alt));

	GtkTreeIter  iter;
	GtkTreeSelection *selection = gtk_tree_view_get_selection(to_treeview);
	GtkTreeModel *model = GTK_TREE_MODEL(fps_list_store);
	int c_ind = -1;
	if( gtk_tree_selection_get_selected(selection, &model, &iter) ) {
		// get data from current row
		gtk_tree_model_get (GTK_TREE_MODEL(fps_list_store), &iter, FPS_IND_COLUMN, &c_ind, -1);
	}
	if( c_ind < 0 ) c_ind = 0; 
	//cout << "selected client=" << c_ind << endl;

	// clear local plan
	creating_plan = false;

	clear_client_canvas(MAX_CLIENTS);
	wpPlans[MAX_CLIENTS].plan.clear_list();

	// get local client
	Client * me = guiStr.shm_ptr->clients.getClient(c_ind);
	if( !me || !me->state.gps) {
		cout << "-- Did not find client or client does not have GPS yet" << endl;
		return;
	}

	// convert coordinates to ECEF
	CartVector_t me_ecef = lla2ecef( me->telemetry.gps.pos );

	// get ECEF to NED DCM at current location
	Matrix C_e_n = ecef2nedDCM( me->telemetry.gps.pos );

	// TO plan points in a NED frame. with NED frame, create default plan 
	CartVector_t toPlan_ned[6];

	toPlan_ned[0].x = 0;    // point at trailer
	toPlan_ned[0].y = 0;
	toPlan_ned[0].z = -alt; // m up in alt

	toPlan_ned[1].x = height;  // m north
	toPlan_ned[1].y = 0;
	toPlan_ned[1].z = -alt; // m up in altitude

	toPlan_ned[2].x =  height / 2.0; // m north
	toPlan_ned[2].y =  width / 2.0; // m east
	toPlan_ned[2].z = -alt; // m up in altitude

	toPlan_ned[3].x = -height / 2.0; // m south
	toPlan_ned[3].y =  width / 2.0; // m east
	toPlan_ned[3].z = -alt; // m up in altitude

	toPlan_ned[4].x = -height / 2.0; // m south
	toPlan_ned[4].y = -width / 2.0; // m west
	toPlan_ned[4].z = -alt; // m up in altitude

	toPlan_ned[5].x =  height / 2.0; // m north
	toPlan_ned[5].y = -width / 2.0; // m west
	toPlan_ned[5].z = -alt; // m up in altitude

	// convert points to gps plan
	for(int i=0; i < 6; i++ ) {
		Matrix R(3,1);
		R(0) = toPlan_ned[i].x;
		R(1) = toPlan_ned[i].y;
		R(2) = toPlan_ned[i].z;

		// now convert NED points to ECEF frame relative vector
		R = ~C_e_n * R;

		// now translate them by location of trailer in ECEF
		CartVector_t point_ecef;
		point_ecef.x = me_ecef.x + R(0);
		point_ecef.y = me_ecef.y + R(1);
		point_ecef.z = me_ecef.z + R(2);

		// convert the new ECEF location to geodectic lat, lon
		GPSLLA_t gps_point = ecef2lla(point_ecef);

		// convert lat, lon coordinates into waypoint
		GPSWaypoint_t point;
		point.Lat = gps_point.lat;
		point.Lon = gps_point.lon;
		point.Alt = gps_point.alt;

		// set waypoint ordering
		point.index = i + index;
		if( i == 5 )
			point.Next  = 2 + index;
		else
			point.Next  = i + 1 + index;

		// save waypoint in plan
		wpPlans[MAX_CLIENTS].plan.set_waypoint(point);
	}
	// now draw the new plan
	create_canvas_flightplan(MAX_CLIENTS);
}

void FPManager::create_default_lost_comm_plan()
{
	if( guiStr.builder == NULL ) {
		cout << "create_default_lost_comm_plan() - no builder for gui" << endl;
		return;
	}

	// create window and setup signals
	if( !lc_win ) {
		lc_win    = GTK_WIDGET (gtk_builder_get_object (guiStr.builder, "lostcomm_dialog"));
		lc_index  = GTK_WIDGET (gtk_builder_get_object (guiStr.builder, "lc_index"));
		lc_radius = GTK_WIDGET (gtk_builder_get_object (guiStr.builder, "lc_radius"));
		lc_dir    = GTK_WIDGET (gtk_builder_get_object (guiStr.builder, "lc_dir"));
		lc_alt    = GTK_WIDGET (gtk_builder_get_object (guiStr.builder, "lc_alt"));
		lc_sw     = GTK_WIDGET (gtk_builder_get_object (guiStr.builder, "lc_client_vsw"));

		if( !lc_win || !lc_index || !lc_radius || !lc_alt || !lc_dir || !lc_sw) {
			cout << "Invalid xml ui file for default lost-comm dialog" << endl;
			return;
		}
		// add the dialog response buttons
		gtk_dialog_add_button(GTK_DIALOG(lc_win), GTK_STOCK_CANCEL, 1);
		gtk_widget_grab_focus( gtk_dialog_add_button(GTK_DIALOG(lc_win), GTK_STOCK_OK, 2) );

		// create list view 
		lc_treeview = create_fps_tree(lc_treeview, lc_sw);
	}

	// update the list with all clients
	update_client_list();

	int retval = gtk_dialog_run(GTK_DIALOG(lc_win)); 
	gtk_widget_hide(GTK_WIDGET(lc_win));

	if( retval != 2 ) {
		return;
	}

	// get values from dialog
	int index    = (int)gtk_spin_button_get_value(GTK_SPIN_BUTTON(lc_index));
	float radius = gtk_spin_button_get_value(GTK_SPIN_BUTTON(lc_radius));
	float alt    = gtk_spin_button_get_value(GTK_SPIN_BUTTON(lc_alt));
	bool dir     = gtk_toggle_button_get_active(GTK_TOGGLE_BUTTON(lc_dir));

	GtkTreeIter  iter;
	GtkTreeSelection *selection = gtk_tree_view_get_selection(lc_treeview);
	GtkTreeModel *model = GTK_TREE_MODEL(fps_list_store);
	int c_ind = -1;
	if( gtk_tree_selection_get_selected(selection, &model, &iter) ) {
		// get data from current row
		gtk_tree_model_get (GTK_TREE_MODEL(fps_list_store), &iter, FPS_IND_COLUMN, &c_ind, -1);
	}
	if( c_ind < 0 ) c_ind = 0; 
	//cout << "selected client=" << c_ind << endl;

	// clear local plan
	creating_plan = false;
	clear_client_canvas(MAX_CLIENTS);
	wpPlans[MAX_CLIENTS].plan.clear_list();

	// get client
	Client * me = guiStr.shm_ptr->clients.getClient(c_ind);
	if( !me || !me->state.gps) {
		cout << "-- Did not find client or client does not have GPS yet" << endl;
		return;
	}

	// convert coordinates to ECEF
	CartVector_t me_ecef = lla2ecef( me->telemetry.gps.pos );

	// get ECEF to NED DCM at current location
	Matrix C_e_n = ecef2nedDCM( me->telemetry.gps.pos );

	// TO plan points in a NED frame. with NED frame, create default plan 
	CartVector_t toPlan_ned[2];

	toPlan_ned[0].x = 0;    // oribtal point at trailer
	toPlan_ned[0].y = 0;
	toPlan_ned[0].z = -alt; // m up in alt

	toPlan_ned[1].x = 0;  // m north
	toPlan_ned[1].y = 2*radius; // m east
	toPlan_ned[1].z = -alt; // m up in altitude

	// convert points to gps plan
	for(int i=0; i < 2; i++ ) {
		Matrix R(3,1);
		R(0) = toPlan_ned[i].x;
		R(1) = toPlan_ned[i].y;
		R(2) = toPlan_ned[i].z;

		// now convert NED points to ECEF frame relative vector
		R = ~C_e_n * R;

		// now translate them by location of trailer in ECEF
		CartVector_t point_ecef;
		point_ecef.x = me_ecef.x + R(0);
		point_ecef.y = me_ecef.y + R(1);
		point_ecef.z = me_ecef.z + R(2);

		// convert the new ECEF location to geodectic lat, lon
		GPSLLA_t gps_point = ecef2lla(point_ecef);

		// convert lat, lon coordinates into waypoint
		GPSWaypoint_t point;
		point.Lat = gps_point.lat;
		point.Lon = gps_point.lon;
		point.Alt = gps_point.alt;

		// set waypoint ordering
		point.index = i + index;
		if( i == 0 ) {
			point.Next           = 1 + index;
			point.OrbitRadius    = radius;
			point.OrbitDirection = dir;

		} else {
			point.OrbitRadius = 0;
			point.Next        = index;
		}

		// save waypoint in plan
		wpPlans[MAX_CLIENTS].plan.set_waypoint(point);
	}
	// now draw the new plan
	create_canvas_flightplan(MAX_CLIENTS);
}

void FPManager::update_client_list()
{
	// update the list with all clients
	GtkTreeIter iter;
	int c_ind = 0;
	Client * a_client = NULL;
	while( (a_client = guiStr.shm_ptr->clients.getClient(c_ind) ) ) {

			//-- walk through the list looking for the link in the tree model
			// if empty, just fill in new value
			bool found = false;
			bool valid = gtk_tree_model_get_iter_first( GTK_TREE_MODEL(fps_list_store), &iter);
			while( valid ) {
				int link_ind;
				gtk_tree_model_get (GTK_TREE_MODEL(fps_list_store), &iter, FPS_IND_COLUMN, &link_ind, -1);
				// if this is the current link
				if( link_ind == c_ind ){
					found = true;
					break;
				}

				valid = gtk_tree_model_iter_next ( GTK_TREE_MODEL(fps_list_store), &iter);
			}

			// if we still have not found the client, then add it
			if( !found ) {
				gtk_list_store_append (fps_list_store, &iter);

				//-- fill in the gtk tree menu --//
				gtk_list_store_set (fps_list_store, &iter,
						FPS_IND_COLUMN,  c_ind,
						FPS_NAME_COLUMN, a_client->name.c_str(),
						-1);
			}
		
		// look for next client
		c_ind++;
	}
}
